
/****************************************************************************
*						PlanesDetection Class header						*
*    				Copyright (C) 2012  J.R. Ruiz-Sarmiento					*
*																			*
*    This program is free software: you can redistribute it and/or modify	*
*    it under the terms of the GNU General Public License as published by	*
*    the Free Software Foundation, either version 3 of the License, or		*
*    (at your option) any later version.									*
*																			*
*    This program is distributed in the hope that it will be useful,		*
*    but WITHOUT ANY WARRANTY; without even the implied warranty of			*
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the			*
*    GNU General Public License for more details.							*
*																			*
*    You should have received a copy of the GNU General Public License		*
*    along with this program.  If not, see <http://www.gnu.org/licenses/>.	*
*																			*
****************************************************************************/

#ifndef _SORT_MODELS_
#define _SORT_MODELS_

#include <iostream>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <pcl/segmentation/sac_segmentation.h>

using namespace std;

namespace SORT
{
namespace ObjectRecognition
{

	struct CClassification
	{
	public:

		enum Type{ Wall, Floor, Ceiling, Table, Chair, Computer_screen };

		CClassification(){};

		CClassification( const Type &type, const double &confidence ) : m_type( type ), m_confidence ( confidence ) 
						{}

		inline Type getType() const { return m_type; }
		inline double getConfidence() const { return m_confidence; }
		inline void setType( const Type &type ) { m_type = type; }
		inline void setConfidence( const double &confidence ) { m_confidence = confidence; }

	private:

		Type	m_type;
		double	m_confidence;		
	};

	class CModel
	{
	protected:
		float								m_height;
		float								m_width;
	public:
		pcl::PointIndices::Ptr				m_inliers;
		pcl::ModelCoefficients::Ptr			m_coeffs;
		Eigen::Vector3f 					m_centroid;
		pcl::PointCloud<pcl::PointXYZ>::VectorType	m_contour;
		Eigen::Matrix3f 					m_covariance;
		CClassification						m_classification;


		CModel()
		{};

		CModel( pcl::PointIndices::Ptr		&inliers, 
				pcl::ModelCoefficients::Ptr &coeffs,
				Eigen::Vector3f				&centroid,
				pcl::PointCloud<pcl::PointXYZ>::VectorType &contour,
				Eigen::Matrix3f				&covariance)
				: m_inliers(inliers), 
				m_coeffs(coeffs),
				m_centroid(centroid),
				m_contour(contour),
				m_covariance(covariance)
		{}


		inline float getHeight() const { return m_height; }
		inline float getWidth() const { return m_width; }


		inline CClassification::Type getType() const { return m_classification.getType(); }
		inline double getConfidence() const { return m_classification.getConfidence(); }
		inline void setType( const CClassification::Type &type ) { m_classification.setType( type ); }
		inline void setConfidence( const double &confidence ) { m_classification.setConfidence( confidence ); }


	public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	};

	class CPlane: public CModel
	{
	private:
		#define CHECK_IF_REFINED if ( !m_refined ){ refineSlopeToHighLevel(); computeHeightAndWidth(); }
		
	public:	

		enum TPlaneType{ VERTICAL=0, ALMOST_VERTICAL, HORIZONTAL, ALMOST_HORIZONTAL, DIAGONAL, UNKNOWN } m_type;	

		CPlane():CModel()
		{
			m_id = ID_PLANE++;
			m_refined = false;
		};
		
		/*	Constructor
		 */
		CPlane(pcl::PointIndices::Ptr		&inliers, 
				pcl::ModelCoefficients::Ptr &coeffs,
				Eigen::Vector3f				&centroid,
				pcl::PointCloud<pcl::PointXYZ>::VectorType &contour,
				Eigen::Matrix3f				&covariance,
				const float					&elongation = 0,
				const TPlaneType			&type = CPlane::UNKNOWN ): CModel(inliers,coeffs,centroid,contour,covariance)
													, m_elongation ( elongation )
													, m_type( type )
		{
			m_id = ID_PLANE++;

			// Refine slope to a high level value: VERTICAL, HORIZONTAL...
			refineSlopeToHighLevel	();

			// Compute plane's height and Width
			computeHeightAndWidth	();

			// Check if elongation is provided, compute if not
			if ( !elongation )
				computeElongation		();

			// Ok, this plane has been refined!
			m_refined = true;
		}

		/*	Overloaded << operator
		 */
		friend ostream& operator<<(ostream& output, const CPlane& c)
		{ 
			// ID_PLANE Orientation centroid_x centroid_y centroid_z size(area)
			output << c.getId();

			if (c.m_type == TPlaneType::VERTICAL )
				output << " VERTICAL";
			if (c.m_type == TPlaneType::ALMOST_VERTICAL )
				output << " ALMOST_VERTICAL";
			if ( c.m_type == TPlaneType::HORIZONTAL )
				output << " HORIZONTAL";
			if ( c.m_type == TPlaneType::ALMOST_HORIZONTAL )
				output << " ALMOST_HORIZONTAL";
			if ( c.m_type == TPlaneType::DIAGONAL )
				output << " DIAGONAL";

			// Store coordinates in the MRPT coordinate' system
			//                 
			//			   z ^	   ^
			//				 |    / x
			//				 |   /
			//				 |  /
			//				 | /
			//   y <---------|/
			//
			output << " " << c.m_centroid[2]; // has_centroid_x
			output << " " << -c.m_centroid[0]; // has_centroid_y put the y axis pointing up
			output << " " << -c.m_centroid[1]; // has_centroid_z

			output << " " << c.getHeight() * c.getWidth();
			output << " " << c.getElongation();
			
			return output; 
		}

		void print()
		{
			CHECK_IF_REFINED

			cout << "---------- plane-" << m_id << " ----------"  << endl;
			cout << "Num of inliers: " << m_inliers->indices.size() << endl;
			cout << "Coefs: " << *(m_coeffs);
			cout << "Type: ";

			if ( m_type == TPlaneType::VERTICAL )
				cout << "VERTICAL" << endl;
			if ( m_type == TPlaneType::ALMOST_VERTICAL )
				cout << "ALMOST_VERTICAL" << endl;
			if ( m_type == TPlaneType::HORIZONTAL )
				cout << "HORIZONTAL" << endl;
			if ( m_type == TPlaneType::ALMOST_HORIZONTAL )
				cout << "ALMOST_HORIZONTAL" << endl;
			if ( m_type == TPlaneType::DIAGONAL )
				cout << "DIAGONAL" << endl;

			cout << "Centroid (x,y,z): (" << m_centroid[0] << ", " << m_centroid[1] << ", " << m_centroid[2] << ")" << endl;
			cout << "Height: " << m_height << " Width: " << m_width << endl;
		}

		inline void refine()
		{
			if ( !m_refined ){ computeHeightAndWidth(); refineSlopeToHighLevel(); }
			m_refined = true;
		}

		int getId() const { return m_id; }
		float getElongation( ) const { return m_elongation; }

	protected:		

		std::string refineSlopeToHighLevel();

		float computeVerticalSlope() const;		

		void computeElongation();
		
	private:

		static int	ID_PLANE; 
		int			m_id;
		bool		m_refined;		
		float		m_elongation;

		void computeHeightAndWidth();

	public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	};
}
}

#endif
